#include <FastSerial.h>
#define CAMERAPORT Serial2
#define IMUPORT Serial1
#define DEBUGPORT Serial
#define BUFFERSIZE 32
FastSerialPort0(DEBUGPORT);
FastSerialPort1(IMUPORT);
FastSerialPort2(CAMERAPORT);

#include <Servo.h>
Servo ROLLSERVO;
Servo PITCHSERVO;
Servo TILTSERVO;
Servo PANSERVO;
#define ROLLPIN 22
#define PITCHPIN 23
#define TILTPIN 24
#define PANPIN 25
#define ROLLMAX 2500
#define ROLLMIN -2500
#define PITCHMAX 4000
#define PITCHMIN -4000
#define TILTMAX 4500
#define TILTMIN -4500
#define PANMAX 17900
#define PANMIN -18000
#define POSMAX 2100
#define POSMIN 900
#define VELMAX 1550
#define VELMIN 1450

//interrupts
#define INT4PIN 2
#define INT5PIN 3
#define INT1PIN 20
#define INT0PIN 21
#define ICP4PIN 49
#define ICPNORM 49
#define INTNORM 108

#define IMUPIN 8
#define LEDPIN 13
#define VCCPIN 53 //hall effect
#define GNDPIN 51 //hall effect

//int0
volatile unsigned int timer1INT0;
volatile unsigned int timer2INT0;
volatile byte flagINT0;
ISR(INT0_vect)
{
  if (EICRA & B00000001) {//if we captured rising
    EICRA = EICRA ^ B00000001;//falling edge
    timer1INT0=TCNT4;
    flagINT0=0;
  }
  else {
    EICRA = EICRA ^ B00000001;//rising edge
    timer2INT0=TCNT4;
    flagINT0=1;
  }
}

//int1
volatile unsigned int timer1INT1;
volatile unsigned int timer2INT1;
volatile byte flagINT1;
ISR(INT1_vect)
{
  if (EICRA & B00000100) {//if we captured rising
    EICRA= EICRA ^ B00000100;//falling edge
    timer1INT1=TCNT4;
    flagINT1=0;
  }
  else {
    EICRA=EICRA ^ B00000100;//rising edge
    timer2INT1=TCNT4;
    flagINT1=1;
  }
}

//ICP4
volatile byte flagICP4;
ISR(TIMER4_CAPT_vect)
{
  if (!digitalRead(ICP4PIN)) {// magnet
    TCCR4B|=B01000000;//set rising edge
    flagICP4=1;
  }
  else { //no magnet
    TCCR4B&=B10111111;//set falling edge
    flagICP4=0;
  }
}


byte DEBUGreceive(char* buffer) {
  if (DEBUGPORT.available() > 0) {
    byte i;
    for (i=0;i<BUFFERSIZE;i++) {
      while (DEBUGPORT.available() == 0);
      buffer[i]=DEBUGPORT.read();
      if (buffer[i]=='!') {//LF
      if (buffer[i]=='!') {//LF
      //if (buffer[i]==0x0D) {//CR
        return (i+1);
      }
    }
  }
  return 0;
}
          
      
byte IMUreceive(byte* buffer) {
  if (IMUPORT.available() >= 6) { //4prefix+size+ID
    if (IMUPORT.read()=='D') { //PRE1
      if (IMUPORT.read()=='I') { //PRE2
        if (IMUPORT.read()=='Y') { //PRE3
          if (IMUPORT.read()=='d') { //PRE4
            buffer[0]=IMUPORT.read(); //payload size
            if (buffer[0]>BUFFERSIZE-2){//if detected size is too large for buffer
              IMUPORT.flush();
              return 0;
            }
            buffer[1]=IMUPORT.read(); //Message ID
            while (IMUPORT.available()<buffer[0]);
            byte i;
            for (i = 2; i < buffer[0]+2 ; i++)
              buffer[i] = IMUPORT.read();
            IMUPORT.flush(); //ignore cksum
            return 1; //message available
          }
        }
      }
    }
  }
  return 0; //no message available
}

void setup()
{
  pinMode(LEDPIN, OUTPUT);  //LED debug
  pinMode(IMUPIN, OUTPUT); //groundstart
  digitalWrite(IMUPIN,HIGH); //dont groundstart
  DEBUGPORT.begin(57600);
  IMUPORT.begin(38400);
  CAMERAPORT.begin(9600);
  
  //timer4 setup
  TCCR4A=0;
  TCCR4B=B00000001;//prescaler=1
  TCCR4C=0;
  
  //servo setup
  ROLLSERVO.attach(ROLLPIN);
  PITCHSERVO.attach(PITCHPIN);
  PANSERVO.attach(PANPIN);
  TILTSERVO.attach(TILTPIN);
  
  //pan setup
  pinMode(VCCPIN, OUTPUT);  //hall effect
  pinMode(GNDPIN, OUTPUT); //hall effect
  digitalWrite(GNDPIN,LOW);//hall effect
  digitalWrite(VCCPIN,HIGH);//hall effect
  
  //intial positions
  PITCHSERVO.writeMicroseconds(900);
  ROLLSERVO.writeMicroseconds(900);
  TILTSERVO.writeMicroseconds(1500);
  PANSERVO.writeMicroseconds(1500);  

  //int0 ISR for pan signal
  pinMode(INT0PIN, INPUT); //pan signal from APM
  digitalWrite(INT0PIN,HIGH); //internal pullup
  EICRA|=B00000011;//rising INT0
  EIMSK|=B00000001;//enable INT0
  
  //int1 ISR for tilt signal
  pinMode(INT1PIN, INPUT); //tilt signal from APM
  digitalWrite(INT1PIN,HIGH); //internal pullup
  EICRA|=B00001100;//rising INT1
  EIMSK|=B00000010;//enable INT1

  //ICP4 ISR for hall effect
  pinMode(ICP4PIN, INPUT); //hall effect
  digitalWrite(ICP4PIN,HIGH); //internal pullup
  TCCR4B|=B11000000;//ICP filter plus rising
  TIMSK4|=B00100000;//ICP enable  


} 


int bytes2int(byte* buffer)
{
  union bytes2int {
    byte b[2];
    int i;
  } bytes2int;
  bytes2int.b[0]=buffer[0];
  bytes2int.b[1]=buffer[1];
  return(bytes2int.i);
}

long bytes2long(byte* buffer)
{
  union bytes2long {
    byte b[4];
    long l;
  } bytes2long;
  bytes2long.b[0]=buffer[0];
  bytes2long.b[1]=buffer[1];
  bytes2long.b[2]=buffer[2];
  bytes2long.b[3]=buffer[3];
  return(bytes2long.l);
}
  
void loop()
{
  //IMU msg variables   
  byte IMUbuffer[BUFFERSIZE];
  char DEBUGbuffer[BUFFERSIZE];
  byte CAMERAbuffer[BUFFERSIZE];
  byte msgID;
  
  //IMU variables
  int roll;
  int pitch;
  int yaw;
  
  //pan variables
  int panpos;
  
  //Servo commands
  unsigned int rollcmd=1500;
  unsigned int pitchcmd=1500;
  unsigned int pancmd=1500;
  unsigned int tiltcmd=1500;
  byte deployed=0;
  
  //GPS variables
  long longitude=0;
  long latitude=0;
  int altitude=0;
  int ground_speed=0;
  int ground_course=0;
  long gpstime=0;
  byte fix;
  
  //Misc variables
  byte i,j;
  byte light=LOW;
  unsigned long t=millis();
  int temp;
  byte zoom=0x00;
  byte buffsize;
  
   while(1){
    
    if (flagICP4) {// magnet
      panpos=0;
    }
    
    if (flagINT0) {
      pancmd=(timer2INT0-timer1INT0)>>4;
      pancmd=pancmd-6;
      if (deployed) {
        //PANSERVO.writeMicroseconds(pancmd);
      }
      flagINT0=0;   
    }
    
    if (flagINT1) {
      tiltcmd=(timer2INT1-timer1INT1)>>4;
      tiltcmd=tiltcmd-18;
      if (deployed) {
        //TILTSERVO.writeMicroseconds(tiltcmd);
      }
      flagINT1=0;
    }    
    
    if(IMUreceive(IMUbuffer)){
      msgID=IMUbuffer[1];
      if (msgID==0x02){ //Attitude packet
        roll=bytes2int(&IMUbuffer[2]);
        pitch=bytes2int(&IMUbuffer[4]);
        yaw=bytes2int(&IMUbuffer[6]);
        
        roll=constrain(roll,ROLLMIN,ROLLMAX);
        pitch=constrain(pitch,PITCHMIN,PITCHMAX);
        
        rollcmd=map(roll,ROLLMIN,ROLLMAX,POSMIN,POSMAX);
        pitchcmd=map(pitch,PITCHMIN,PITCHMAX,POSMIN,POSMAX);
        
        if (deployed) {
          ROLLSERVO.writeMicroseconds(rollcmd);
          PITCHSERVO.writeMicroseconds(pitchcmd);
        }


      } else if (msgID==0x03){//GPS packet
        fix=IMUbuffer[20];
        if (fix) {
          longitude=bytes2long(&IMUbuffer[2]);
          latitude=bytes2long(&IMUbuffer[6]);
          altitude=bytes2int(&IMUbuffer[10]);
          ground_speed=bytes2int(&IMUbuffer[12]);
          ground_course=bytes2int(&IMUbuffer[14]);
          gpstime=bytes2long(&IMUbuffer[16]);
        }
      } else { //unknown packet
        DEBUGPORT.println("Invalid Packet");
      }//end packet detection
    } //end receive event
    
    if (CAMERAPORT.available()>0) {
      CAMERAbuffer[j++]=CAMERAPORT.read();
      if (CAMERAbuffer[j-1]==0xFF) {
        DEBUGPORT.write(CAMERAbuffer,j);
        DEBUGPORT.println();
        j=0;
      }
    }        
    
    buffsize=DEBUGreceive(DEBUGbuffer);
    if (buffsize) {
      //DEBUGPORT.write((byte *)DEBUGbuffer,buffsize);
      DEBUGPORT.print(DEBUGbuffer);
      if (DEBUGbuffer[0]=='!') {//command
        if (DEBUGbuffer[1]=='P' && DEBUGbuffer[2]=='A') {
          temp=atoi(&DEBUGbuffer[3]);
          temp=constrain(temp,POSMIN,POSMAX);
          if (deployed) {
            PANSERVO.writeMicroseconds(temp);
          }
        }
        if (DEBUGbuffer[1]=='T' && DEBUGbuffer[2]=='I') {
          temp=atoi(&DEBUGbuffer[3]);
          temp=constrain(temp,POSMIN,POSMAX);
          if (deployed) {
            TILTSERVO.writeMicroseconds(temp);
          }
        }      
        if (DEBUGbuffer[1]=='R' && DEBUGbuffer[2]=='E') {
          deployed=0;
          ROLLSERVO.writeMicroseconds(1500);
          PITCHSERVO.writeMicroseconds(1500);
          TILTSERVO.writeMicroseconds(1500);
          PANSERVO.writeMicroseconds(1500);
        }       
        if (DEBUGbuffer[1]=='D' && DEBUGbuffer[2]=='E') {
          deployed=1;
          ROLLSERVO.writeMicroseconds(1500);
          PITCHSERVO.writeMicroseconds(1500);
          TILTSERVO.writeMicroseconds(1500);
          PANSERVO.writeMicroseconds(1500);
        }       
        if (DEBUGbuffer[1]=='G' && DEBUGbuffer[2]=='S') {
          digitalWrite(IMUPIN,LOW); //do a groundstart
        }
        if (DEBUGbuffer[1]=='C' && DEBUGbuffer[2]=='A') {
          if (DEBUGbuffer[3]=='Z' && DEBUGbuffer[3]=='+') {
            if (zoom!=7) zoom++;
            CAMERAbuffer[0]=0x81;
            CAMERAbuffer[1]=0x01;
            CAMERAbuffer[2]=0x04;
            CAMERAbuffer[3]=0x47;
            CAMERAbuffer[4]=zoom;
            CAMERAbuffer[5]=0x00;
            CAMERAbuffer[6]=0x00;
            CAMERAbuffer[7]=0x00;
            CAMERAbuffer[8]=0xFF;
            CAMERAPORT.write(CAMERAbuffer,9);
          }
          if (DEBUGbuffer[3]=='U' && DEBUGbuffer[3]=='-') {
            if (zoom!=0) zoom--;
            CAMERAbuffer[0]=0x81;
            CAMERAbuffer[1]=0x01;
            CAMERAbuffer[2]=0x04;
            CAMERAbuffer[3]=0x47;
            CAMERAbuffer[4]=zoom;
            CAMERAbuffer[5]=0x00;
            CAMERAbuffer[6]=0x00;
            CAMERAbuffer[7]=0x00;
            CAMERAbuffer[8]=0xFF;
            CAMERAPORT.write(CAMERAbuffer,9);          
          } 
          if (DEBUGbuffer[3]=='O' && DEBUGbuffer[4]=='N') {
            CAMERAbuffer[0]=0xF8;
            CAMERAbuffer[1]=0x30;
            CAMERAbuffer[2]=0x01;
            CAMERAbuffer[3]=0xFF;
            CAMERAPORT.write(CAMERAbuffer,4); // set address
            CAMERAbuffer[0]=0x81;
            CAMERAbuffer[1]=0x01;
            CAMERAbuffer[2]=0x00;
            CAMERAbuffer[3]=0x01;
            CAMERAbuffer[4]=0xFF;
            CAMERAPORT.write(CAMERAbuffer,5); // clear cmd buffer
            CAMERAbuffer[0]=0x81;
            CAMERAbuffer[1]=0x01;
            CAMERAbuffer[2]=0x04;
            CAMERAbuffer[3]=0x00;
            CAMERAbuffer[4]=0x02;
            CAMERAbuffer[5]=0xFF;
            CAMERAPORT.write(CAMERAbuffer,6); // CAM on  
            CAMERAbuffer[0]=0x81;
            CAMERAbuffer[1]=0x01;
            CAMERAbuffer[2]=0x04;
            CAMERAbuffer[3]=0x15;
            CAMERAbuffer[4]=0x02;
            CAMERAbuffer[5]=0xFF;
            CAMERAPORT.write(CAMERAbuffer,6); // disp on    
          }                    
        } 
      }      
    }//end receive
    
    if( (millis()-t) > 100) { // Main loop runs at 10Hz
      t=millis();
      light=!light;
      digitalWrite(13, light);   // set the LED on
      
      //DEBUGPORT.print(rollcmd);DEBUGPORT.print("\t");
      //DEBUGPORT.print(pitchcmd);DEBUGPORT.print("\t");
      //DEBUGPORT.print(pancmd);DEBUGPORT.print("\t");
      //DEBUGPORT.print(tiltcmd);DEBUGPORT.print("\t");
      //DEBUGPORT.print(flagICP4);DEBUGPORT.print("\t");
      //DEBUGPORT.print(deployed);DEBUGPORT.print("\t");      
      //DEBUGPORT.println();
      
    }//end 10Hz
    
  }//end while(1)
}//end loop()
